#include <cmath>

#include "lshape.h"

namespace perception
{
    namespace algorithm
    {

        LShape::LShape()
        {
        }
        LShape::~LShape()
        {
        }

        float LShape::Compute(std::vector<data_cell> &lidar_objects)
        {
            // 找到四个点，xy最大最小值，然后取y，然后找到y最大最小与其值向量的夹角90度的值
            // 1.找到四个点，xy最大最小值
            auto f = GetMaxMinXY(lidar_objects);
            // 2.取最大距离的角点，
            auto t = GetMaxCorner(f);
            // 3.遍历所有点，找到向量夹角为90度附近的点集，取距离两点最长点
            point min_x;

            // 4.三点形成一个包围框，得到长宽及航向
            if (GetCornerPoints(lidar_objects, t[0], t[1], min_x))
                return GetHeading(t[0], t[1], min_x);

            else
                return 500;
        }
        std::vector<point> LShape::GetMaxCorner(std::vector<point> &f)
        {
            std::vector<point> t;
            t.resize(2);
            float dis = 0;
            for (int i = 0; i < f.size() - 1; i++)
            {
                for (int j = i + 1; j < f.size(); j++)
                {
                    float dis_tmp = std::sqrt(std::pow(f[i].x - f[j].x, 2) + std::pow(f[i].y - f[j].y, 2));
                    if (dis_tmp > dis)
                    {
                        dis = dis_tmp;
                        t[0] = f[i];
                        t[1] = f[j];
                    }
                }
            }
            return t;
        }
        std::vector<point> LShape::GetMaxMinXY(std::vector<data_cell> &cells)
        {
            point min_y;
            point max_y;
            point min_x;
            point max_x;
            min_y.y = 100;
            max_y.y = -100;
            min_x.x = 100;
            max_x.x = -100;
            for (auto &cell : cells)
            {
                if (cell.x < min_x.x)
                {
                    min_x.x = cell.x;
                    min_x.y = cell.y;
                }

                if (cell.x > max_x.x)
                {
                    max_x.x = cell.x;
                    max_x.y = cell.y;
                }

                if (cell.y < min_y.y)
                {
                    min_y.x = cell.x;
                    min_y.y = cell.y;
                }
                if (cell.y > max_y.y)
                {
                    max_y.x = cell.x;
                    max_y.y = cell.y;
                }
            }
            std::vector<point> four_points;
            four_points.emplace_back(min_x);
            four_points.emplace_back(max_x);
            four_points.emplace_back(min_y);
            four_points.emplace_back(max_y);
            return four_points;
        }
        bool LShape::GetCornerPoints(std::vector<data_cell> &cells, point &max_y, point &min_y, point &result)
        {
            std::vector<point> tmp_point;
            std::vector<float> dis;
            auto ABC = GetABC(max_y, min_y);
            // 找出满足正负阈值+90度的点
            for (auto &min_x : cells)
            {
                // 求角度
                float maxy_x = std::sqrt(std::pow(max_y.x - min_x.x, 2) + std::pow(max_y.y - min_x.y, 2));
                float miny_x = std::sqrt(std::pow(min_y.x - min_x.x, 2) + std::pow(min_y.y - min_x.y, 2));
                // 跳过自身的点
                if (maxy_x < 0.01 || miny_x < 0.01)
                    continue;
                float maxy_miny = std::acos(((max_y.x - min_x.x) * (min_y.x - min_x.x) +
                                             (max_y.y - min_x.y) * (min_y.y - min_x.y)) /
                                            (maxy_x * miny_x)) *
                                  90 / M_PI_2;
                // if ((min_y.x < 11.5 && min_y.x > 6 && min_y.y > -6 && min_y.y < -3) ||
                //     (max_y.x < 11.5 && max_y.x > 6 && max_y.y > -6 && max_y.y < -3))
                // {
                //     std::cout << "min_y:" << min_y.x << " " << min_y.y << std::endl;
                //     std::cout << "max_y:" << max_y.x << " " << max_y.y << std::endl;
                //     std::cout << "min_x:" << min_x.x << " " << min_x.y << std::endl;
                //     std::cout << maxy_miny << " " << maxy_x << " " << miny_x << std::endl;
                // }

                if (maxy_miny > 90 - th_rad && maxy_miny < 90 + th_rad)
                {
                    point t;
                    t.x = min_x.x;
                    t.y = min_x.y;
                    tmp_point.emplace_back(t);

                    float dis_;
                    if (std::fabs(ABC[0] < 0.01))
                    {
                        dis_ = std::fabs(min_x.y);
                    }
                    else if (std::fabs(ABC[1] < 0.01))
                    {
                        dis_ = std::fabs(min_x.x); 
                    }
                    else
                    {
                        dis_ = (ABC[0] * min_x.x + ABC[1] * min_x.y + ABC[2]) / (std::sqrt(std::pow(ABC[0], 2) + std::pow(ABC[1], 2)));
                    }
                    dis.emplace_back(dis_);
                }
            }
            // 找出这些点中距离最大的点
            if (tmp_point.empty())
            {
                std::cout << "没找到角点" << std::endl;
                return false;
            }
            else
            {
                float tmp_dis = 0;
                for (int i = 0; i < tmp_point.size(); i++)
                {
                    if (dis[i] > tmp_dis)
                    {
                        result = tmp_point[i];
                        tmp_dis = dis[i];
                    }
                }
            }
            // std::cout << "min_y:" << min_y.x << " " << min_y.y << std::endl;
            // std::cout << "max_y:" << max_y.x << " " << max_y.y << std::endl;
            // std::cout << "min_x:" << result.x << " " << result.y << std::endl;
            return true;
        }
        std::vector<float> LShape::GetABC(point &a, point &b)
        {
            float A;
            float B;
            if (std::fabs(a.y - b.y) < 0.01)
            {
                A = 0;
                B = -1;
            }
            else if (std::fabs(a.x - b.x) < 0.01)
            {
                B = 0;
                A = std::numeric_limits<float>::max();
            }
            else
            {
                A = (a.y - b.y) / (a.x - b.x);
                B = -1;
            }
            float C = -(A * a.x - a.y);

            std::vector<float> ABC;
            ABC.emplace_back(A);
            ABC.emplace_back(B);
            ABC.emplace_back(C);
            return ABC;
        }
        float LShape::GetHeading(point &max_y, point &min_y, point &min_x)
        {
            if ((min_y.x < 11.5 && min_y.x > 6 && min_y.y > -6 && min_y.y < -3) ||
                (max_y.x < 11.5 && max_y.x > 6 && max_y.y > -6 && max_y.y < -3))
            {
                std::cout << "min_y:" << min_y.x << " " << min_y.y << std::endl;
                std::cout << "max_y:" << max_y.x << " " << max_y.y << std::endl;
                std::cout << "min_x:" << min_x.x << " " << min_x.y << std::endl;
            }
            float heading;
            float maxy_x = std::sqrt(std::pow(max_y.x - min_x.x, 2) + std::pow(max_y.y - min_x.y, 2));
            float miny_x = std::sqrt(std::pow(min_y.x - min_x.x, 2) + std::pow(min_y.y - min_x.y, 2));
            if (maxy_x > miny_x)
                heading = std::atan2(max_y.y - min_x.y, max_y.x - min_x.x);
            else
                heading = std::atan2(min_y.y - min_x.y, min_y.x - min_x.x);
            return heading;
        }

    } // namespace perception
} // namespace avos